/*
 * Copyright (c) 2010 Network Security Lab (NSL), Department of Electrical
 *                    Engineering, University of Washington
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation;
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * Author: He Wu mdzz@uw.edu
 */

/**
 * @file localization.c
 */

#include <string.h>
#include <unistd.h>
#include <math.h>
#include <stdio.h>
#include <time.h>
#include <pthread.h>
#include "command_def.h"
#include "command.h"
#include "router.h"
#include "localization.h"
#include "network.h"

// debug switch
#define local_debug 1

/******************************************************************************/

static __u16 localAddr;         //!< local node address
//
static beacon b[MAX_ANCHORS];   //!< list of RSSI measurements of anchors
static int resume_localization; //!< flag for resuming localization from crash
static int propagate;           //!< propagation flag
static int loc_status;          //!< localization flag
static int anchor;              //!< anchor flag
static float assignedX;         //!< assigned X location
static float assignedY;	        //!< assigned Y location
static float estX;		          //!< estimated X location
static float estY;		          //!< estimated Y location
static int iteration_count;     //!< # of iterations for localization
static int anchor_count;        //!< # of anchor nodes locator can hear
static int reset_b_list;        //!< flag for resetting b list
static grid g;                  //!< grid for gird score
static pathLossModel p;         //!< path loss model for weighted grid score
//
static int max_rssi_samples;    //!< max sample period (NOTICE must be less than MAX_RSSI_BEACON)
static int locator_interval;    //!< localization timer interval (seconds)
static int anchor_interval;     //!< anchor timer interval (seconds)
//
static pthread_mutex_t rssi_list_lock = PTHREAD_MUTEX_INITIALIZER;  //!< RSSI measurement array lock

/******************************************************************************/

/**
 * Initializes localization config file.
 */
static int init_local_config();

/**
 * Update localization config file.
 */
static int update_local_config();

/**
 * Initialize localization parameters.
 */
static void init_localization();

/**
 * Resets neighbor RSSI measurement list.
 */
static void reset_b();

/**
 * Sends anchor broadcast.
 */
static int anchor_broadcast ();

/**
 * Send location result packet back to base node. ACK enabled.
 */
static int location_report ();

/**
 * @brief Updates the list of RSSI measurements based on anchor packet.
 * @param x X coordinate of the anchor.
 * @param y Y coordinate of the anchor.
 * @param rssi RSSI measurement.
 * @param sender_addr Anchor address.
 */
static void update_rssi_list (float x, float y, __s8 rssi, __u16 sender_addr);

/******************************************************************************/

static int init_local_config()
{
  FILE *fp;

  // open local config file
  fp = fopen("/root/local.conf", "r");
  if (fp == NULL) {
    printf("local.conf doesn't exist, creating...\n");
    if (update_local_config() != 0) // write default values to config file
      return -1;
  }
  else {
    fscanf(fp, "%d\n", &resume_localization);
    // load previously crashed config
    if (resume_localization) {
      printf("Recovering from crash!\n");

      fscanf(fp, "%d\n", &propagate);
      fscanf(fp, "%d\n", &loc_status);
      fscanf(fp, "%d\n", &anchor);
      fscanf(fp, "%f\n", &assignedX);
      fscanf(fp, "%f\n", &assignedY);
      fscanf(fp, "%f\n", &estX);
      fscanf(fp, "%f\n", &estY);
      fscanf(fp, "%d\n", &max_rssi_samples);
      fscanf(fp, "%d\n", &locator_interval);
      fscanf(fp, "%d\n", &anchor_interval);

      if (local_debug) {
        printf("\npropagate = %d\n", propagate);
        printf("loc_status = %d\n", loc_status);
        printf("anchor = %d\n", anchor);
        printf("assignedX = %f\n", assignedX);
        printf("assignedY = %f\n", assignedY);
        printf("estX = %f\n", estX);
        printf("estY = %f\n", estY);
        printf("max_rssi_samples = %d\n", max_rssi_samples);
        printf("locator_interval = %d\n", locator_interval);
        printf("anchor_interval = %d\n", anchor_interval);
      }

      // sanity check
      if (!max_rssi_samples) max_rssi_samples = MAX_RSSI_SAMPLE;
      if (!locator_interval) locator_interval = LOCATOR_INTERVAL;
      if (!anchor_interval) anchor_interval = ANCHOR_INTERVAL;
    } // end if(resume_localization)
    else fclose(fp);
  } // end else
  return 0;
}


static int update_local_config()
{
	FILE *fp;
	// write local config to file
	fp = fopen("/root/local.conf", "w");
	if (fp == NULL)
		return -1;
	else {
		// write current config to file
		fprintf(fp, "%d\n", resume_localization);
		fprintf(fp, "%d\n", propagate);
		fprintf(fp, "%d\n", loc_status);
		fprintf(fp, "%d\n", anchor);
		fprintf(fp, "%f\n", assignedX);
		fprintf(fp, "%f\n", assignedY);
		fprintf(fp, "%f\n", estX);
		fprintf(fp, "%f\n", estY);
		fprintf(fp, "%d\n", max_rssi_samples);
		fprintf(fp, "%d\n", locator_interval);
		fprintf(fp, "%d\n", anchor_interval);
		fclose(fp);
	}
	return 0;
}


static void init_localization()
{
  resume_localization = 0;
  propagate = 0;
  loc_status = 0;
  anchor = 0;
  assignedX = 0;
  assignedY = 0;
  estX = 0;
  estY = 0;
  iteration_count = 0;
  anchor_count = 0;
  reset_b_list = 1; // reset list in first run
  max_rssi_samples = MAX_RSSI_SAMPLE;
  locator_interval = LOCATOR_INTERVAL;
  anchor_interval = ANCHOR_INTERVAL;
  // setup grid
  g.xmax = estX + GRID_SIZE;
  g.xmin = estX - GRID_SIZE;
  g.ymax = estY + GRID_SIZE;
  g.ymin = estY - GRID_SIZE;
  g.stepp = GRID_STEP;
  // setup path loss model
  p.alpha = ALPHA;
  p.k = K;
  p.sigdB = SIGDB;
}


static void reset_b()
{
  int i, j;
  anchor_count = 0;
  for (i = 0; i < MAX_ANCHORS; i++) {
    b[i].addr = BROADCAST_ADDR;
    b[i].age = 0;
    b[i].RSSIindex = 0;
    b[i].d = 0;
    b[i].x = 0;
    b[i].y = 0;
    for (j = 0; j < MAX_RSSI_BEACON; j++)
      b[i].RSSI[j] = MIN_RSSI - 1;  // set to 1 below minimum value
  }
}


static int anchor_broadcast ()
{
  packet out_pkt;
  initPkt(&out_pkt, ANCHOR_BROADCAST);  // pkt initialization
  out_pkt.data[0] = 0;  // set rand to 0
  out_pkt.data[1] = LOC_DATA;
  memcpy(out_pkt.data + 2, &localAddr, 2);  // local address
  memcpy(out_pkt.data + 4, &assignedX, 4);  // x location
  memcpy(out_pkt.data + 8, &assignedY, 4);  // y location
  // broadcast location packet, no ACK
  printf("\nBroadcasting anchor location!\n");
  return h_send(BROADCAST_ADDR, &out_pkt, 0);
}


static int location_report ()
{
  packet out_pkt;
  initPkt(&out_pkt, TO_BASE_DATA_PKT_TYPE); // pkt initialization
  out_pkt.data[0] = 0;  // set rand to 0
  out_pkt.data[1] = EST_LOC_REPORT;
  memcpy(out_pkt.data + 2, &localAddr, 2);  // local address
  memcpy(out_pkt.data + 4, &estX, 4);       // x location
  memcpy(out_pkt.data + 8, &estY, 4);       // y location
  return sendtoBase(&out_pkt, 1);
}

static void update_rssi_list (float x, float y, __s8 rssi, __u16 sender_addr)
{
  int i;
  pthread_mutex_lock(&rssi_list_lock);
  // scan all entries
  for (i = 0; i < MAX_ANCHORS; i++) {
    // update existing entry
    if (b[i].addr == sender_addr) {
      // reset age
      b[i].age = 0;
      // wrapping around
      if (b[i].RSSIindex >= max_rssi_samples)
         b[i].RSSIindex = 0; // wrap around
      // assign x, y location
      b[i].x = x;
      b[i].y = y;
      // record RSSI
      b[i].RSSI[b[i].RSSIindex] = rssi;
      b[i].RSSIindex++;
      //
      if (local_debug) {
        printf("\nLocation data received from %x, X: %f, Y: %f\n", sender_addr, b[i].x, b[i].y);
        printf("RSSI is: %d\n\n", b[i].RSSI[b[i].RSSIindex]);
      }
      break;  // skip the rest of the list
    }
    // adds new entry
    else if (b[i].addr == BROADCAST_ADDR) {
      if (local_debug)
        printf("Neighbor added: %x\n", sender_addr);
      b[i].RSSIindex = 0;
      b[i].addr = sender_addr;
      // assign x, y location
      b[i].x = x;
      // record RSSI
      b[i].RSSI[b[i].RSSIindex] = rssi;
      b[i].RSSIindex++;
      // update # of neighbors one can hear
      anchor_count++;
      if (anchor_count >= MAX_ANCHORS)
        anchor_count = MAX_ANCHORS;
      //
      if (local_debug) {
        printf("\nLocation data received from %x, X: %f, Y: %f\n", sender_addr, b[i].x, b[i].y);
        printf("RSSI is: %d\n\n", b[i].RSSI[b[i].RSSIindex]);
      }
      break;  // skip the rest of the list
    }
  } // end for
  pthread_mutex_unlock(&rssi_list_lock);
}


/*
// Reports anchor location back to base node. ACK enabled.
static int anchor_location_report ()
{
  packet out_pkt;
  initPkt(&out_pkt, TO_BASE_DATA_PKT_TYPE); // pkt initialization
  out_pkt.data[0] = 0;  // set rand to 0
  out_pkt.data[1] = ACTUAL_LOC_REPORT;      // report actual location
  memcpy(out_pkt.data + 2, &localAddr, 2);  // local address
  memcpy(out_pkt.data + 4, &estX, 4);       // x location
  memcpy(out_pkt.data + 8, &estY, 4);       // y location
  printf("\nSending location data back to base.\n\n");
  return sendtoBase(&out_pkt, 1); // report actual location back to base
}
*/

/******************************************************************************/

void localization_msg(packet* in_pkt)
{
  __u16 sender_addr;
  float x, y; // anchor location

  if (local_debug)
    printf("\nLocalization handler started!\n");

  // change max_rssi_samples
  if (in_pkt->data[1] == CHANGE_MAX_RSSI) {
    max_rssi_samples = in_pkt->data[4];   // max_rssi_samples is unsigned char, in data[4]
    if (max_rssi_samples >= MAX_RSSI_BEACON)
      max_rssi_samples = MAX_RSSI_BEACON;
    update_local_config();  // update config file
    return;
  }

  // change LOCAL_TIME_INTERVAL
  if (in_pkt->data[1] == CHANGE_LOCAL_TIMER_INTERVAL) {
    locator_interval = (int)(in_pkt->data[4]); // LOCATOR_INTERVAL is unsigned char, in data[4]
    update_local_config();  // update config file
    return;
  }

  // turn on/off propagation
  if (in_pkt->data[1] == PROPOGATE_ON) {
    propagate = 1;
    update_local_config();  // update local config file
    return;
  }
  if (in_pkt->data[1] == PROPOGATE_OFF) {
    propagate = 0;
    update_local_config();  // update local config file
    return;
  }

  // turn on locationing
  if (in_pkt->data[1] >= LOC_ON) {
    loc_status = in_pkt->data[1];
    resume_localization = 1;  // set resume flag
    update_local_config();    // update local config file
    return;
  }

  // Turn off locationing
  if (in_pkt->data[1] == LOC_OFF) {
    init_localization();
    update_local_config();    // update local config file
    off_green();  // turn-off green LED
    off_red();    // turn-off red LED
    on_blue();    // make sure blue is on
    return;
  }

  // anchor the mote at an x,y location
  // sets the location and turn off the timer
  if(in_pkt->data[1] == ASSIGN_XY) {
    memcpy(&assignedX, in_pkt->data + 4, 4);  // assign X location
    memcpy(&assignedY, in_pkt->data + 8, 4);  // assign Y location
    estX = assignedX; //
    estY = assignedY; //
    anchor = 1;   // set anchor flag
    resume_localization = 1;  // set resume flag
    update_local_config();    // update local config file
    off_red();
    off_green();
    on_blue();
    if (local_debug)
      printf("\nAnchored at X = %f, Y = %f\n\n", assignedX, assignedY);
    return;
  }

  // localization packet
  if (loc_status) {
    sender_addr = in_pkt->senderAddr;  // get sender address
    // process received locationing packets from anchors
    if (in_pkt->data[1] == LOC_DATA && !anchor) {
      if (local_debug)
        printf("Location received from: %x\n", sender_addr);
      // get anchor location
      memcpy(&x, (in_pkt->data + 4), 4);
      memcpy(&y, (in_pkt->data + 8), 4);
      // update RSSI measurement list
      update_rssi_list(x, y, in_pkt->rss, sender_addr);
    }
  }
}


void *local_timer()
{
	float tempRSSI;
	int i, j;
	int rssi_count;

	// get local address
	localAddr = get_addr();
	
	// initialize
	init_localization();

  // initialize from localization config file
  if (init_local_config() != 0)
    printf("Failed to initialized localization config file!\n");

	// main loop
	while (1) {
    // debug display
    if (local_debug && !check_base()) {	// not showing for base node
      printf("\nLocalization Timer!\n");
      printf("iteration_count = %d\n", iteration_count);
      printf("anchor_count = %d\n", anchor_count);
      printf("anchor = %d\n", anchor);
      printf("loc_status = %d\n\n", loc_status);
    }

    // initialize b list
    if (reset_b_list) {
      reset_b();
      reset_b_list = 0;	// reset flag
    }

    // do nothing if localizing is turned off
    if (!loc_status) {
      usleep(anchor_interval * US_TO_SEC);  // use anchor interval
      continue;
    }

    // update iteration count
    iteration_count++;

    // for anchor nodes
    if (anchor) {
      toggle_green();     // toggle green LED for anchor node
      anchor_broadcast(); // broadcast anchor location
#ifdef ENABLE_ANCHOR_LOCATION_REPORT
      iteration_count++;  // update counter
      // send location data back to base every MAX_WAIT * local_timer_interval seconds
      if (iteration_count >= MAX_WAIT) {
        iteration_count = 0;	// reset iteration_count
        anchor_location_report();
      }
#endif
      usleep(skew(anchor_interval * US_TO_SEC));
      continue;
    }

    // for localizing nodes
    toggle_red();  // blink red for localizing node

    // run localization algorithms
    if ((iteration_count >= max_rssi_samples) && (anchor_count != 0)) {
      if (local_debug)
        printf("Numbers of neighbors = %d\n", anchor_count);

      // calculate distance to anchor using average RSSI values
      for (i = 0; i < anchor_count; i++) {
        tempRSSI = 0;
        rssi_count = 0;
        for (j = 0; j < MAX_RSSI_BEACON; j++) {
          if (b[i].RSSI[j] > (__s8)MIN_RSSI) {
            tempRSSI += b[i].RSSI[j];
            rssi_count++;
          }
        }
        tempRSSI = tempRSSI / (float)rssi_count;
        // calculate the distance to that anchor
        b[i].d = RSSI_to_distance(tempRSSI, K, ALPHA);
        if (local_debug)
          printf("Distance = %f \tRSSI: %f \tFrom node: %x \tAge = %d\n",
                 b[i].d, tempRSSI, b[i].addr, b[i].age);
      }

      iteration_count = 0;	// reset iteration counter

      /*
       * Entering critical section. The list of RSSI measurements is protected.
       */
      pthread_mutex_lock(&rssi_list_lock);

      // run centroid
      if(loc_status == CENTROID) {
        printf("\nComputing Location using centroid method.\n");
        //
        centroid(b, anchor_count, &estX, &estY);
        if (local_debug)
          printf("Result of centroid is: X = %f, Y = %f\n", estX, estY);
      }
      // run weighted centroid
      else if (loc_status == WEIGHTEDCENTROID) {
        printf("\nComputing Location using Weighted Centroid method.\n");
        //
        weightedCentroid(b, anchor_count, &estX, &estY);
        if (local_debug)
          printf("Result of centroid is: X = %f, Y = %f\n", estX, estY);
      }
      // run gridscore
      else if (loc_status == GRIDSCORE) {
        printf("\nComputing Location using Grid Score method.\n");
        weightedCentroid(b, anchor_count, &estX, &estY);
        if (local_debug)
          printf("Result of Weighted-Cetroid in Gridscore is: X = %f, Y = %f\n", estX, estY);
        //
        voting(b, anchor_count, g, GRID_ERROR_THRESHOLD, &estX, &estY);
        if (local_debug)
          printf("Result of Gridscore is: X = %f, Y = %f\n", estX, estY);
      }
      // run gradient
      else if (loc_status == GRADIENT) {
        printf("\nComputing Location using Gradient method.\n");
        //
        gradient(b, anchor_count, DELTA, &estX, &estY);
        if (local_debug)
          printf("Result of Gradient is: X = %f, Y = %f\n", estX, estY);
      }
      // run weighted grid score
      else if (loc_status == WEIGHTED_SCORE) {
        printf("\nComputing Location using Weighted Grid Score method.\n");
        weightedCentroid(b, anchor_count, &estX, &estY);
        if (local_debug)
          printf("Result of Weighted-Centroid in Gridscore is: X = %f, Y = %f\n", estX, estY);
        //
        w_voting(b, anchor_count, g, p, &estX, &estY);
        if (local_debug)
          printf("Result of Weighted Grid Score is: X = %f, Y = %f\n", estX, estY);
      }
      // run weighted gradient
      else if (loc_status == WEIGHTED_GRADIENT) {
        printf("\nComputing Location using Weighted Gradient method.\n");
        weightedCentroid(b, anchor_count, &estX, &estY);
        if (local_debug)
          printf("Result of Weighted-Centroid in Weighted Gradient is: X = %f, Y = %f\n", estX, estY);
        //
        w_gradient(b, anchor_count, DELTA, &estX, &estY);
        if (local_debug)
          printf("Result of Weighted Gradient is: X = %f, Y = %f\n", estX, estY);
      }

      // age RSSI samples
      for(i = 0; i < MAX_ANCHORS; i++) {
        b[i].age++;
        // check age
        if (b[i].age >= ANCHOR_AGE_LIMIT) {
          /*
           * TODO Re-implement b as linked list
           */
          reset_b();
        }
      }

      /*
       * Leaving critical section.
       */
      pthread_mutex_unlock(&rssi_list_lock);

      location_report();      // report location back to base node
      update_local_config();  // update local config file

    } // end if ((iteration_count >= max_rssi_samples) && (anchor_count != 0))

    // reset counter if no anchor is heard
    if ((iteration_count >= max_rssi_samples) && (anchor_count == 0))
      iteration_count = 0;

#ifdef ENABLE_PROPAGATION
    // propagate location
    if (propagate) {
      packet out_pkt;
      // build location packet
      out_pkt.data[0] = 0;	// set rand to 0
      out_pkt.data[1] = LOC_DATA;
      memcpy(out_pkt.data + 2, &localAddr, 2);  // local address
      memcpy(out_pkt.data + 4, &estX, 4);       // estimated x location
      memcpy(out_pkt.data + 8, &estY, 4);       // estimated y location
      // send location packet, no ACK
      initPkt(&out_pkt, ANCHOR_BROADCAST);      // packet initialization
      h_send(BROADCAST_ADDR, &out_pkt, 0);
    }
#endif

    usleep(skew(locator_interval * US_TO_SEC));
	}	// end while(1)
}


/*******************************************************************************
 *   RSSI2Distance Converts RSSI measurements into distance estimates
 *   Copyright (C) 2009  Jeffrey Vander Stoep jevander@u.washington.edu
 *
 *   This program is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 3 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *   d = (k/10^(RSS/10))^(1/alpha)
 *   input RSSI, k and alpha.  See CC2420 datasheet on converting RSSI
 *   to received power.
 ******************************************************************************/
float RSSI_to_distance(float RSSI, float k, float alpha)
{
	float d = 0.0;
	float Pr = 0.0;
	float Pr_dBm = 0.0;
	Pr_dBm = (RSSI - 45.0);    // As per the CC2420 data sheet from RSSI to dBm
	Pr = pow(10.0, Pr_dBm/10); // From dBm to watts
	//Pt = Pt_t[Transmit_level];
	d = pow(k/Pr, 1.0/alpha);
	return(d);
}

/*******************************************************************************
 * Title: centroid
 *
 * Description: This localization algorithm estimates location
 *        as the center of gravity of the beacons within radio range.
 *        Does not use RSSI measurements.
 * Author: Jeff Vander Stoep jvanderstoep@gmail.com
 *
 ******************************************************************************/
void centroid(beacon b[MAX_ANCHORS], int num_b, float *estX, float *estY)
{
	int i;
	float x = 0.0;
	float y = 0.0;

	for(i = 0; i < num_b; i++)
	{
		x = x + (b[i].x);
		y = y + (b[i].y);
	}
	x = x / num_b;
	y = y / num_b;
	*estX = x;
	*estY = y;
	return;
}

/*******************************************************************************
 * Title: weightedCentroid
 *
 * Description: This localization algorithm estimates location as the center of
 *        gravity of the beacons within radio range, weighting each beacons
 *        location (or its gravitational pull) by the inverse of the estimated
 *        distance to that beacon. Distance estimates can be done using a
 *        RSSI and a pathloss model.
 * Author: Jeff Vander Stoep jvanderstoep@gmail.com
 *
 * Here is what's going on. Three different distances are estimated (d1, d2, d3)
 *      along with the coordinates (x1, x2, x3) and (y1, y2, y3) the three
 *      beacons. The x coordinate is calculated using as
 *      x = ((1/d1)*x1 + (1/d2)*x2 + (1/d3)*x3) * (1/(1/d1 + 1/d2 + 1/d3))
 *      where  (1/(1/d1 + 1/d2 + 1/d3)) normalizes the weighting
 *
 ******************************************************************************/
void weightedCentroid(beacon b[MAX_ANCHORS], int num_b, float *estX, float *estY)
{
   int i;
   float x = 0;
   float y = 0;
   float total_weight = 0;

   for(i = 0; i < num_b; i++ )
   {
	   //add up the 1/d multiples to be used for normalizing
	   total_weight = total_weight + 1/b[i].d;
	   x = x + ((b[i].x) / b[i].d);
	   y = y + ((b[i].y) / b[i].d);
   }
   *estX = ((1 / total_weight) * x);
   *estY = ((1 / total_weight) * y);
   return;
}


/*******************************************************************************
 * Title: voting - basic
 *
 * Description: This is a primitive algorithm for grid scoring with the method
 * of intersecting annulli. It is not optimized for speed/memory.
 * Author: Jeff Vander Stoep
 *
 * ****************************************************************************/
void voting(beacon b[MAX_ANCHORS], int num_b, grid g, float errorThreshold, float *estX, float *estY)
{
	int i;
	float x,y;
	int score;
	int MaxScore = 0;
	int MaxScoreIndex = 0;
	float computedDistance;
	float minDistanceBound, maxDistanceBound;
	float *XofCellsWithMaxScore;
	float *YofCellsWithMaxScore;
	int xlen, ylen;

	xlen = floor((g.xmax-g.xmin)/g.stepp + 1);
	ylen = floor((g.ymax-g.ymin)/g.stepp + 1);

	if ((XofCellsWithMaxScore = (float*) malloc(sizeof(float)*xlen*ylen+1)) == NULL)
		printf("X malloc failed\r\n");
	if ((YofCellsWithMaxScore = (float*) malloc(sizeof(float)*xlen*ylen+1)) == NULL)
		printf("Y malloc failed\r\n");

	// walk the entire grid
	for (x = g.xmin; x <= g.xmax; x += g.stepp)
	{
		for (y = g.ymin; y <= g.ymax; y += g.stepp)
		{
			score = 0;
			for (i = 0; i < num_b; i++)
			{
				computedDistance = sqrt(pow((x - b[i].x),2) + pow((y - b[i].y),2));
				minDistanceBound = ((b[i].d - errorThreshold) > 0) ? (b[i].d - errorThreshold) : 0;
				maxDistanceBound = b[i].d + errorThreshold;
				if((minDistanceBound <= computedDistance) && (computedDistance <= maxDistanceBound))
				{
					score += 1;
				}
			}
			if (score >= MaxScore)
			{
				// new maximum score
				if (score > MaxScore)
				{
					MaxScore = score;
					XofCellsWithMaxScore[0] = x;
					YofCellsWithMaxScore[0] = y;
					MaxScoreIndex = 1;
				}
				// new cell with maximum score
				else
				{
					XofCellsWithMaxScore[MaxScoreIndex] = x;
					YofCellsWithMaxScore[MaxScoreIndex] = y;
					MaxScoreIndex++;
				}
			}
		}
	}
	x = 0;
	y = 0;

	for(i = 0; i < MaxScoreIndex; i++)
	{
		x += XofCellsWithMaxScore[i]/MaxScoreIndex;
		y += YofCellsWithMaxScore[i]/MaxScoreIndex;
	}
	*estX = x;
	*estY = y;

	if (local_debug)
	  printf("MAX Score = %d\n", MaxScore);

	free(XofCellsWithMaxScore);
	free(YofCellsWithMaxScore);
}

/*******************************************************************************
 * Title: weighted voting (gird score)
 *
 * Description: This is a weighted version of voting method. Here, both error
 * threshold and vote are weighted.
 * Author: Jeff Vander Stoep
 *
 * ****************************************************************************/
void w_voting(beacon b[MAX_ANCHORS], int num_b, grid g, pathLossModel pl, float *estX, float *estY)
{
	int i;
	float x,y;
	float score;
	float MaxScore = 0;
	int MaxScoreIndex = 0;
	float weight;
	float computedDistance;
	float minDistanceBound, maxDistanceBound;
	float * XofCellsWithMaxScore;
	float * YofCellsWithMaxScore;
	float std; //Standard deviation
	int xlen, ylen;
	float c;
	xlen = floor((g.xmax-g.xmin)/g.stepp+1);
	ylen = floor((g.ymax-g.ymin)/g.stepp+1);

	if ((XofCellsWithMaxScore = (float*) malloc(sizeof(float)*xlen*ylen+1)) == NULL)
		printf("X malloc failed\r\n");
	if ((YofCellsWithMaxScore = (float*) malloc(sizeof(float)*xlen*ylen+1)) == NULL)
		printf("Y malloc failed\r\n");
	c = (pl.sigdB*LOG10)/(10*pl.alpha);	// 90% confidence interval
	for (x = g.xmin; x <= g.xmax; x += g.stepp)
	{
		for (y = g.ymin; y <= g.ymax; y += g.stepp)
		{
			score = 0;
			for (i = 0; i < num_b; i++)
			{
				computedDistance = sqrt(pow((x - b[i].x),2) + pow((y - b[i].y),2));
				std = c*b[i].d;
				minDistanceBound = ((b[i].d - std) > 0) ? (b[i].d - std) : 0;
				maxDistanceBound = b[i].d + std;
				weight = 1/b[i].d;
				if((minDistanceBound <= computedDistance) && (computedDistance <= maxDistanceBound))
				{
					score+= weight;
				}
			}
			if (score >= MaxScore)
			{
				// new maximum score
				if (score > MaxScore)
				{
					MaxScore = score;
					XofCellsWithMaxScore[0] = x;
					YofCellsWithMaxScore[0] = y;
					MaxScoreIndex = 1;
				}
				// new cell with maximum score
				else
				{
					XofCellsWithMaxScore[MaxScoreIndex] = x;
					YofCellsWithMaxScore[MaxScoreIndex] = y;
					MaxScoreIndex++;
				}
			}
		}
	}
	x = 0;
	y = 0;
	for( i = 0; i < MaxScoreIndex; i++)
	{
		x += XofCellsWithMaxScore[i]/(MaxScoreIndex);
		y += YofCellsWithMaxScore[i]/(MaxScoreIndex);
	}
	*estX = x;
	*estY = y;
	free(XofCellsWithMaxScore);
	free(YofCellsWithMaxScore);
}


/*******************************************************************************
 * Title: gradient
 *
 * Description: This is the gradient descent algorithm. Computes the
 *        least-mean-square (LMS), then the derivative of the LMS.
 *        Finally it moves towards minimizing the error by moving
 *        towards the minimum of the gradient by delta*gradient.
 * Author: Jeffrey Vander Stoep jevander@u.washington.edu
 *
 *******************************************************************************/
void gradient(beacon b[MAX_ANCHORS], int num_b, float delta, float * estX, float * estY)
{
  int i;
  static int IterationNumber = 0;
  float oldX, oldY;
  float x,y;
  float tempX, tempY;
  float Xgradient = 0, Ygradient = 0;
  float neighborX, neighborY;
  float computedDistance;
  float mindistance; // for initializing to nearest neighbor
  int mindist_index = 0;

  if (IterationNumber == 0)
  {
    mindistance = b[0].d;
    for (i = 1; i< num_b; i++)
    {
      if (b[i].d < mindistance)
      {
        mindistance = b[i].d;
        mindist_index = i;
      }
    }
    x = b[mindist_index].x + 0.1;
    y = b[mindist_index].y + 0.1;
    IterationNumber++;
  }
  else
  {
    oldX = *estX;
    oldY = *estY;
    for(i=0;i < num_b; i++)
    {
      neighborX = b[i].x;
      neighborY = b[i].y;
      computedDistance = sqrt(pow((oldX - neighborX),2) + pow((oldY - neighborY),2));
      Xgradient = Xgradient + (computedDistance - b[i].d)* ((oldX - neighborX))/computedDistance;
      Ygradient = Ygradient + (computedDistance - b[i].d)* ((oldY - neighborY))/computedDistance;
    }
    tempX = ((oldX - (delta * Xgradient)));// > 0);// ? (oldX - (ALPHA * Xgradient)) : 0;
    tempY = ((oldY - (delta * Ygradient)));// > 0);// ? (oldY - (ALPHA * Ygradient)) : 0;

    x = tempX;
    y = tempY;
  }
  *estX = x;
  *estY = y;
}


/*******************************************************************************
 * Title: gradient
 *
 * Description: This is the gradient descent algorithm. Computes the
 *        least-mean-square (LMS), then the derivative of the LMS.
 *        Finally it moves towards minimizing the error by moving
 *        towards the minimum of the gradient by delta*gradient.
 * Author: Jeffrey Vander Stoep jevander@u.washington.edu
 *
 ******************************************************************************/
void w_gradient(beacon b[MAX_ANCHORS], int num_b, float delta, float * estX, float * estY)
{
	int i;
	static int IterationNumber = 0;
	float oldX, oldY;
	float x,y;
	float tempX, tempY;
	float Xgradient = 0, Ygradient = 0;
	float neighborX, neighborY;
	float computedDistance;
	float mindistance; //for initializing to nearest neighbor
	float weight;
	int mindist_index = 0;

	if (IterationNumber ==0)
	{
		mindistance = b[0].d;
		for (i = 1; i< num_b; i++)
		{
			if (b[i].d < mindistance)
			{
				mindistance = b[i].d;
				mindist_index = i;
			}
		}
		x = b[mindist_index].x + 0.1;
		y = b[mindist_index].y + 0.1;
		IterationNumber++;
	}
	else
	{
		oldX = *estX;
		oldY = *estY;
		for(i=0;i < num_b; i++)
		{
			neighborX = b[i].x;
			neighborY = b[i].y;
			weight = 1.0/b[i].d;
			computedDistance = sqrt(pow((oldX - neighborX),2) + pow((oldY - neighborY),2));
			Xgradient = Xgradient + weight*(computedDistance - b[i].d)* ((oldX - neighborX))/computedDistance;
			Ygradient = Ygradient + weight*(computedDistance - b[i].d)* ((oldY - neighborY))/computedDistance;
		}
		tempX = ((oldX - (delta * Xgradient)));// > 0);// ? (oldX - (ALPHA * Xgradient)) : 0;
		tempY = ((oldY - (delta * Ygradient)));// > 0);// ? (oldY - (ALPHA * Ygradient)) : 0;
		x = tempX;
		y = tempY;
	}
	*estX = x;
	*estY = y;
}
